% A*算法求解最优路径
close all;clear;clc;
rng(1234);
% 参数初始化
mazeHeight = 50;
mazeWidth = 50;
% 定义迷宫环境
% 使用0表示墙壁，1表示可通行区域，2表示起点，3表示终点，4表示途经点  
maze = Maze(mazeWidth,mazeHeight);
mazeData = maze.generate();
mazeMapInit = maze.mazeMap;
maze.draw();
% 运行选项
showFlag = 0;% 显示运行过程
recordFlag = 0;% 保存运行过程   
% 保存为.mp4视频
if recordFlag && showFlag
    v = VideoWriter('./result/Astar_Diagonal1.mp4', 'MPEG-4');
    v.FrameRate = 60;
    open(v);
end
if recordFlag && showFlag
    % 记录帧
    frame = getframe(gcf);
    writeVideo(v, frame);
end
% 获取开始和结束索引  
startPoint = find(maze.mazeData(:) == 2);
goalPoint = find(maze.mazeData(:) == 3);
transitPointState = find(maze.mazeData(:) == 4);
% 获取开始和结束二维索引
[startRow,startCol] = ind2sub([mazeWidth,mazeHeight], startPoint);
[goalRow,goalCol] = ind2sub([mazeWidth,mazeHeight], goalPoint);
% 需要探索和已探索表
openList = startPoint;
closeList = [];
% 探索节点初始化
node = zeros(1,2);
son = 1;
parent = 2;
node(son) = startPoint;
node(parent) = -1;
baseCost = zeros(mazeWidth*mazeHeight,1);% 起点到当前节点的代价
% 颜色map定义
maze.mazeMap = [[0 0 1;0 1 1];mazeMapInit];
while ~isempty(openList)
    % 选择新节点
    totalCost = [];
    for i = 1:length(openList)
        [row,col] = ind2sub([mazeWidth,mazeHeight], openList(i));
        dx = abs(row - goalRow);dy = abs(col - goalCol);
        predCost = dx +dy + (sqrt(2)-2) * min(dx,dy);% 终点预测代价
        totalCost = [totalCost; baseCost(openList(i)) + predCost];% 代价
%         totalCost = [totalCost; predCost];% 代价
    end
    % 相同代价随机选择
    index = find(totalCost == min(totalCost));
    minCostIndex = index(randi(length(index)));
    currentNode = openList(minCostIndex);
    % 到达终点或探索节点大于理论最大值 退出
    if currentNode == goalPoint || length(closeList) > mazeWidth*mazeHeight
        break;
    end
    closeList = [closeList;openList(minCostIndex)];
    openList(minCostIndex) = [];
    % 当前节点位置
    [row,col] = ind2sub([mazeWidth,mazeHeight], currentNode);
    % 当当前节点不是出发点时绘制当前节点
    if currentNode ~= startPoint && showFlag
        maze.mazeData(ind2sub(size(maze.mazeData), currentNode)) = -1;
        maze.draw();
        titleStr=sprintf("已探索方块数：%d",length(closeList));
        title(titleStr);        
        drawnow;
        if recordFlag
            % 记录帧
            frame = getframe(gcf);
            writeVideo(v, frame);
        end
    end
    relation = zeros(1,2);
    % 临近节点探索
    % 向右移动
    [openList,baseCost,node] = neighborSearch(maze,[row+1,col],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向左移动
    [openList,baseCost,node] = neighborSearch(maze,[row-1,col],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向上移动
    [openList,baseCost,node] = neighborSearch(maze,[row,col-1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向下移动
    [openList,baseCost,node] = neighborSearch(maze,[row,col+1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向右上移动
    [openList,baseCost,node] = neighborSearch(maze,[row+1,col-1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向右下移动
    [openList,baseCost,node] = neighborSearch(maze,[row+1,col+1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向左上移动
    [openList,baseCost,node] = neighborSearch(maze,[row-1,col-1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 向左下移动
    [openList,baseCost,node] = neighborSearch(maze,[row-1,col+1],[row,col],...
                openList,closeList,baseCost,node,son,parent,goalPoint);
    % 绘制临近节点图
    if showFlag
        maze.draw();
        titleStr=sprintf("已探索方块数：%d",length(closeList));
        title(titleStr);
        drawnow;
        if recordFlag
            % 记录帧
            frame = getframe(gcf);
            writeVideo(v, frame);
        end
    end
end
if recordFlag && showFlag
    % 插入3秒的停留帧
    for i = 1:round(60*3)
        % 记录帧
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end
pause(3);
% 寻找最短路径并绘制
clf;
mazePath = goalPoint;% 终点
currentNode = goalPoint;% 从终点开始回溯寻找
for i = 1:length(node(:,1))
    parentRow = find(node(:,son) == currentNode);% 当前节点父节点所在行
    if ~isempty(parentRow)
        if currentNode == startPoint % 寻找到起点停止寻找
            break
        end
        parentNode = node(parentRow,parent);% 当前节点父节点
        mazePath = [parentNode;mazePath];% 记录当前节点父节点
        currentNode = parentNode;% 迭代寻找路径
    else
        break
    end
end
% 绘制结果图
maze.mazeData = mazeData;
maze.mazeMap = [[0 0 1;0 1 1];mazeMapInit];
[currentRow,currentCol] = ind2sub([mazeWidth,mazeHeight], mazePath(1));
[nextRow,nextCol] = ind2sub([mazeWidth,mazeHeight], mazePath(2));
distence = norm([currentRow,currentCol] -[nextRow,nextCol],2);
for i = 2:length(mazePath)-1
    [currentRow,currentCol] = ind2sub([mazeWidth,mazeHeight], mazePath(i));
    [nextRow,nextCol] = ind2sub([mazeWidth,mazeHeight], mazePath(i+1));
    distence = distence + norm([currentRow,currentCol] -[nextRow,nextCol],2);
    if i ~= length(mazePath)-1
        maze.mazeData(currentRow,currentCol) = -1;
        maze.mazeData(nextRow,nextCol) = -2;
    end
    maze.draw();
    titleStr=sprintf("最佳结果:步数%d --- 距离:%0.4f",i,distence);
    title(titleStr);
    drawnow;
    if recordFlag && showFlag
        % 记录帧
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
end
if recordFlag && showFlag
    % 插入3秒的停留帧
    for i = 1:round(60*3)
        % 记录帧
        frame = getframe(gcf);
        writeVideo(v, frame);
    end
    close(v);% 结束保存视频
end
pause(3);
close all;

% 处理邻近位置点数据
function [openListOut,baseCostOut,nodeOut] = neighborSearch(maze,next,current,...
            openList,closeList,baseCost,node,son,parent,goalPoint)
    % 输出数据
    openListOut = openList;
    baseCostOut = baseCost;
    nodeOut = node;
    % 初始化当前节点关系
    relation = zeros(1,2);
    distence = norm(next-current,2);% 移动距离
    % 提取当前和邻近行列
    nextRow = next (1); nextCol= next (2); 
    currentRow = current (1); currentCol = current (2); 
    % 行列未越界
    if nextRow > 0 && nextRow <= maze.mazeWidth && nextCol > 0 && nextCol <= maze.mazeHeight
        % 行列一维表示
        currentNode = sub2ind([maze.mazeWidth,maze.mazeHeight], currentRow,currentCol);
        neighborsNode = sub2ind([maze.mazeWidth,maze.mazeHeight], nextRow,nextCol);
        % 邻近一格不为障碍，相邻块不都为障碍，该格未探索
        if maze.mazeData(nextRow,nextCol) ~= 0 && ~ismember(neighborsNode,closeList) ...
                && (maze.mazeData(nextRow,currentCol) ~= 0 || maze.mazeData(currentRow ,nextCol) ~= 0)
            % 不在探索的行列中
            if ~ismember(neighborsNode,openListOut)
                openListOut = [openListOut;neighborsNode];% 不在探索的行列中
                baseCostOut(neighborsNode) = baseCostOut(currentNode) + distence;% 新建该格到起点的代价
                % 新建父子关系
                relation(son) = neighborsNode;
                relation(parent) = currentNode;
                nodeOut = [nodeOut;relation];% 新建节点父子关系
                % 可视化以探索区域
                if neighborsNode ~= goalPoint
                    maze.mazeData(ind2sub([maze.mazeWidth,maze.mazeHeight], neighborsNode)) = -2;
                end
            end
            % 该格到起点的代价不为最小，更新节点信息信息
            if baseCost(neighborsNode) > baseCostOut(currentNode) + distence
                baseCostOut(neighborsNode) = baseCostOut(currentNode) + distence;% 更新该格到起点的代价
                % 更新父子关系
                index = nodeOut(:,son) == neighborsNode;
                nodeOut(index,parent) = currentNode;
                % 可视化以探索区域
                if neighborsNode ~= goalPoint
                    maze.mazeData(ind2sub([maze.mazeWidth,maze.mazeHeight], neighborsNode)) = -2;
                end
            end
        end
    end
end

